////////////////////////////////////////////////////////////////////////
// This file is part of the simpleSID framework 
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA 
//
// Copyright (c)Richard White, 2009
// See License.txt for a disclaimer of all warranties
// and licensing information

#include <stdafx.h>
#include "Filter.h"

const float Filter::m_PI = 3.1415926535897932384626433832795f;

Filter::Filter()
{
	SetClockFreq(985248.f);
	EnableFiler(true);
	Reset();
}

Filter::~Filter()
{

}

void Filter::Reset()
{
	//These values are for working out the linear realationship between FC and freq
	m_fK = 6.55f;
	m_fB = 20.f;
	
	//zero our registers
	m_byFilt = 0;
	m_byRes = 0;
	m_byMode = 0;
	m_byVol = 0;
	
	m_wFC = 0;
	m_fVol = 0.f;
	m_fRes = 0.f;
	m_fVol = 0.f;
	m_fVhp = m_fVbp = m_fVlp = 0.f;
	
	//Zero our bool versions of the reg's
	m_bFiltV1 = false;
	m_bFiltV2 = false;
	m_bFiltV3 = false;
	m_bFiltExt = false;

	m_bVoice3Off = false;
	m_bLP = false;
	m_bBP = false;
	m_bHP = false;

	m_fW0Cache = 0.f;
	m_f1DivQ = 0.f;
	
	m_fVhp = m_fVbp = m_fVlp = 0.f;

	m_fRes = 0.f;

	SetW0();
	SetQ();
}

void Filter::SetClockFreq(float fClockFreq)
{
	m_fClockFreq = fClockFreq;
}

void Filter::SetFCLo(unsigned char byFCLo)
{
	m_wFC = (m_wFC & 0x7f8) | (byFCLo & 0x007);
	MapFCtoFreq();
	SetW0();
}

void Filter::SetFCHi(unsigned char byFCHi)
{
	m_wFC = ((byFCHi << 3) & 0x7f8) | (m_wFC & 0x007);
	MapFCtoFreq();
	SetW0();
}

void Filter::SetResFilt(unsigned char byResFilt)
{
	m_byRes = (byResFilt >> 4) & 0xf;
	m_fRes = static_cast<float>(m_byRes / 15.f);

	m_byFilt = byResFilt & 0x0f;

	if (m_byFilt & 0x01)
		m_bFiltV1 = true;
	else
		m_bFiltV1 = false;
	if (m_byFilt & 0x02)
		m_bFiltV2 = true;
	else
		m_bFiltV2 = false;
	if (m_byFilt & 0x04)
		m_bFiltV3 = true;
	else
		m_bFiltV3 = false;
	if (m_byFilt & 0x08)
		m_bFiltExt = true;
	else
		m_bFiltExt = false;

	SetQ();
}

void Filter::SetModeVol(unsigned char byModeVol)
{
	m_byMode = (byModeVol & 0xf0) >> 4;

	m_byVol = byModeVol & 0x0f;
	m_fVol = m_byVol / 15.f; //Get the volume as a floating point number

	if (m_byMode & 0x01)
		m_bLP = true;
	else
		m_bLP = false;
	if (m_byMode & 0x02)
		m_bBP = true;
	else
		m_bBP = false;
	if (m_byMode & 0x04)
		m_bHP = true;
	else
		m_bHP = false;
	if (m_byMode & 0x80)
		m_bVoice3Off = true;
	else
		m_bVoice3Off = false;
	
}

void Filter::SetFiltVoice1(bool bState)
{
	m_bFiltV1 = bState;
}

void Filter::SetFiltVoice2(bool bState)
{
	m_bFiltV2 = bState;
}

void Filter::SetFiltVoice3(bool bState)
{
	m_bFiltV3 = bState;
}

void Filter::SetFiltExt(bool bState)
{
	m_bFiltExt = bState;
}

void Filter::SetVoice3Off(bool bState)
{
	m_bVoice3Off = bState;
}

void Filter::SetModeLP(bool bState)
{
	m_bLP = bState;
}

void Filter::SetModeBP(bool bState)
{
	m_bBP = bState;
}

void Filter::SetModeHP(bool bState)
{
	m_bHP = bState;
}

void Filter::SetVol(unsigned char byVol)
{
	m_byVol = byVol & 0x0f;
	m_fVol = m_byVol / 15.f;
}

//For testing only
void Filter::SetFreq(float fFreq)
{
	m_fFreq = fFreq;
	SetW0();
}

void Filter::SetRes(unsigned char byRes)
{
	m_byRes = byRes & 0x0f;
	m_fRes = m_byRes / 15.f;

	SetQ();
}

void Filter::SetW0()
{
	m_fW0Cache = 2.f * m_PI * m_fFreq / m_fClockFreq;
}

void Filter::SetQ()
{
	float Q = m_byRes / 15.f; //Normalise our Q value to 0 - 1
	m_f1DivQ = 1.f / (0.707f + Q * 1.6f);
}

void Filter::MapFCtoFreq()
{
	m_fFreq = m_fK * m_wFC + m_fB; //Change our FC reg into a normal freq setting
}

void Filter::EnableFiler(bool bState)
{
	m_bEnabled = true;
}

//This clocks our filter for one cycle and returns the filtered value
float Filter::Process(int iVoice1, int iVoice2, int iVoice3)
{
	float fVin, fVout; //Input and not filtered input need to talk to alankilla about this
	fVin = fVout = 0.f;
	
	//We assign the voices to the filter or directly to the main output
	if (m_bFiltV1)
		fVin += iVoice1;
	else
		fVout += iVoice1;
	if (m_bFiltV2)
		fVin += iVoice2;
	else
		fVout += iVoice2;
	
	//Voice 3 does not silence by voice3 being off it is still routed throught the filter
	if (m_bFiltV3)
		fVin += iVoice3;
	else if (!m_bVoice3Off)
		fVout += iVoice3;

	//As the external in is not emulated we are just going to pass a 0 in
	if (m_bFiltExt)
		fVin += 0.f;
	else
		fVout += 0.f;

	//Check that  the filter is enabled Take out the voices that are routed to the sid if disabled
	if (!m_bEnabled)
		return (fVout - fVin) * m_fVol;

	//We now do a summing for the filter outputs
	if (m_bLP)
		fVout += m_fVlp;
	if (m_bBP)
		fVout += m_fVbp;
	if (m_bHP)
		fVout += m_fVhp;

	
	m_fVlp += m_fVbp * m_fW0Cache;
	m_fVbp += m_fVhp * m_fW0Cache;
	m_fVhp = -m_fVbp * m_f1DivQ - m_fVlp - fVin;
	
	//Return A scalled filtered output
	return fVout * m_fVol;
}
